 #include <ros/ros.h>
#include <sensor_msgs/JointState.h>
#include <geometry_msgs/Pose.h>
#include <xrobot_control/joint_states.h>
 #include <serial/serial.h>
 #include <queue>

 class JointsStatesController
 {
    #define JOINTS_NUM  xrobot_control::joint_states::JOINTS_NUM
    #define MP_JOINTS_NUM 5
    #define GRIPPER_JOINTS_NUM (JOINTS_NUM-MP_JOINTS_NUM)
 public:
    JointsStatesController(std::string robot, std::string port_name, std::int32_t baudrate = 9600, std::int16_t out_time = 100) 
    : _robot(robot)
    {
        // 串口初始化
        serial::Timeout serial_timeout = serial::Timeout::simpleTimeout(out_time); 
        serial_port.setPort(port_name);
        serial_port.setBaudrate(baudrate);
        serial_port.setTimeout(serial_timeout);

        // 节点初始化
        Node_Init();

    }
    ~JointsStatesController(){};

    /**
     * @brief 打开串口，循环收发数据，与下位机进行通信
     * @param ctrl_Hz 读取串口数据的频率
     */      
    void start(void);
 private:

    // 串口
    serial::Serial serial_port;

    // 关节信息订阅者及发布者
    ros::NodeHandle _node;
    ros::Publisher wxJoints_puber;
    ros::Publisher wxEffector_puber;
    ros::Publisher wxJoyStick_puber;

    // 串口数据存储区
    static const uint16_t REV_LEN = 50; 
    std::uint8_t serial_revbuf[REV_LEN];

    // 关节名称
   std::string joints_name[6] = {"Joint1", "Joint2", "Joint3", "Joint4", "Joint5", "Joint6"};

    // 机器人名字
    std::string _robot;

     /**
     * @brief 开启串口
     */    
    void Open_Serial(void)
    {
        try
        {    
            serial_port.open();
        }
        catch(serial::IOException& e)
        {
            ROS_ERROR("Unable to open the serial port");
        }
        if(serial_port.isOpen()) ROS_INFO("Successfully open the serial port");
    }

    /**
     * @brief 节点初始化
     */    
    void Node_Init(void)
    {
        // 关节空间坐标发布
        wxJoints_puber = _node.advertise<sensor_msgs::JointState>("xrobot_monitor/joints_control", 10);

        // 笛卡尔空间坐标发布
        wxEffector_puber = _node.advertise<geometry_msgs::Pose>("xrobot_monitor/effector_control", 10);

        // 手柄数据发布
        wxJoyStick_puber = _node.advertise<geometry_msgs::Pose>("xrobot_monitor/joystick_control", 10);
    }

    /**
     * @brief 更新关节状态，true 成功更新 false 无更新
     * @note 其中包括了下位机通信，使用前需要确保串口开启
     */     
    bool UpdateData(void)
    {
        std::vector<float> js(JOINTS_NUM);

        if(serial_port.isOpen())
        {
            // 读取串口中有效数据个数
            size_t recv_size = serial_port.available();
            if(recv_size > 0)
            {
                // 读取串口数据
                recv_size = serial_port.read(serial_revbuf, recv_size);

                // 数据处理
                DataHandle(serial_revbuf);
            }
            else return false;
        }
        else return false;
    }

    /**
     * @brief 对小程序发送给来的数据进行解包，并发布
     */     
    void DataHandle(std::uint8_t*  buf)
    {
        std::vector<double> val;

        // 数据解包
        val = Unpack(buf);

       // 发布笛卡尔空间坐标
        if(buf[0]  == '0')
        {
            geometry_msgs::Pose msg;
            msg.position.x = val[0]; msg.position.y = val[1]; msg.position.z = val[2];
            msg.orientation.x = val[4]; msg.orientation.y = val[3]; 
            msg.orientation.w = 0;
            wxEffector_puber.publish(msg);
        }

        // 发布关节空间坐标
        else if(buf[0] == '7')
        {
            geometry_msgs::Pose msg;
            msg.orientation.w = 1;
            msg.position.x = val[0];
            msg.position.y = val[1];
            wxJoyStick_puber.publish(msg);
        }
        else if(buf[0] == '8')
        {
            geometry_msgs::Pose msg;
            msg.orientation.w = 2;
            msg.position.z = val[0];
            wxJoyStick_puber.publish(msg);
        }
        else if(buf[0] == '9')
        {
            geometry_msgs::Pose msg;
            msg.orientation.w = 3;      
            msg.position.z = val[0];
            wxJoyStick_puber.publish(msg);               
        }    
        else
        {
            sensor_msgs::JointState js;
            js.position = val;
            js.velocity.push_back(2);
            js.name.push_back(joints_name[buf[0]-'1']);
            wxJoints_puber.publish(js);
        }
    }

    /**
     * @brief 串口数据解包
     * 
     * @param in    原始数据包
     * @param out   要发布的数据
     */
    std::vector<double> Unpack(std::uint8_t*  in)
    {
        std::vector<double> out;
        double data;
        uint8_t* start  = in + 1;
      
        // 笛卡尔空间
        if(in[0] == '0')
        {
            for(int i = 0; i < 5; i ++)
            {
                data = (*(start+i*3) - '0' ) * 100;
                data += (*(start+1+i*3) - '0' ) * 10;
                data += (*(start+2+i*3) - '0' );
                if(i < 3) data -= 300;
                else data  = (data-180) * 3.14 / 180;
                out.push_back(data);
            }
        }

        else if(in[0] == '7')
        {
            for(int i = 0; i < 2; i ++)
            {
                data = (*(start+i*3) - '0' ) * 100;
                data += (*(start+1+i*3) - '0' ) * 10;
                data += (*(start+2+i*3) - '0' );
                data -= 100;
                out.push_back(data);
            }              
        }

        else if(in[0] == '8' || in[0] == '9')
        {
            data = *start - '0';
            out.push_back(data);
        }

        // 关节空间
        else
        {
            data = (*start - '0' ) * 100;
            data += (*(start+1) - '0' ) * 10;
            data += (*(start+2) - '0' );       
            data  = (data-135) * 3.14 / 180;
            out.push_back(data);
        }
        return out;
    }
};
